#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

float min_range = 0.0;                    // 雷达最小距离
float min_range_angle = 0.0;              // 雷达最小距离对应的角度
float dis_thre=0.6;

// 雷达数据回调函数
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg) {
    // 获取雷达测距数据
    std::vector<float> ranges = scan_msg->ranges;

    // 找到最小距离及其对应的角度
    min_range = *std::min_element(ranges.begin(), ranges.end());
    int min_index = std::min_element(ranges.begin(), ranges.end()) - ranges.begin();
    min_range_angle = scan_msg->angle_min + min_index * scan_msg->angle_increment;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "q4_2");
    ros::NodeHandle nh;
    // 订阅雷达话题
    ros::Subscriber scan_sub = nh.subscribe("/scan", 10, scanCallback);
    // 发布速度控制话题
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
    // 设置循环频率
    ros::Rate rate(2); // 2Hz
    while (ros::ok())
    {
        geometry_msgs::Twist cmd_vel;
        if(min_range<dis_thre)
        {
            if (min_range_angle<0.0)
            {
                cmd_vel.angular.z=0.8;
                cmd_vel.angular.x=0.0;
                cmd_vel.angular.y=0.0;
                cmd_vel.linear.x=0.2;
                cmd_vel.linear.y=0.0;
                cmd_vel.linear.z=0.0;
            }
            else
            {
                cmd_vel.angular.z=-0.8;
                cmd_vel.angular.x=0.0;
                cmd_vel.angular.y=0.0;
                cmd_vel.linear.x=0.2;
                cmd_vel.linear.y=0.0;
                cmd_vel.linear.z=0.0;
            }
        }
        else
        {
            cmd_vel.angular.z=0.0;
            cmd_vel.angular.x=0.0;
            cmd_vel.angular.y=0.0;
            cmd_vel.linear.x=0.2;
            cmd_vel.linear.y=0.0;
            cmd_vel.linear.z=0.0;
        }
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}